/*
 * simplified_svm.c
 *
 *  Created on: Mar 27, 2024
 *      Author: Yiwen Zhu
 */
#include "simplified_svm.h"
#include "Motor_Control_Init.h"

void simplified_svpwm_calc(void){
	_iq S,C,Sd,Cd,Sq,Cq,Uf,Ug,Uao1,Uao,Ubo1,Ubo,Uco1,Uco,Umax,Umin,Umid;

	S = simp_svm1.Sin *simp_svm1.KUdc;
	C = simp_svm1.Cos *simp_svm1.KUdc;

	Sd = simp_svm1.Ud *S;
	Cd = simp_svm1.Ud *C;
	Sq = simp_svm1.Uq *S;
	Cq = simp_svm1.Uq *C;

	Uf = 0.5f*(Sq-Cd);
	Ug = 0.866f*(Sd+Cq);

	// 排序并求平均
	Uao1 = Cd - Sq;
	Umax = Uao1;

	Ubo1 = Uf + Ug;
	if(Ubo1>Umax){
	    Umax = Ubo1;
	    Umin = Uao1;
	}
	else{
	    Umin = Ubo1;
	}

	Uco1 = Uf - Ug;
	if(Uco1>Umax){
	    Umax = Uco1;
	}
	else if (Uco1<Umin){
	    Umin = Uco1;
	}

	Umid = 0.5f*(Umin+Umax);

	// 比较值偏移
	Uao = Uao1 - Umid;
	Ubo = Ubo1 - Umid;
	Uco = Uco1 - Umid;

	simp_svm1.Ta = Uao + 0.5f;
	simp_svm1.Tb = Ubo + 0.5f;
	simp_svm1.Tc = Uco + 0.5f;

}

